﻿/*****************************************************************//**
 * \file   EndToolBoplarHook.cpp
 * \brief  boplar hook算法类
 *
 * \author Administrator
 * \date   August 2021
 * \modify by Administrator
 * \date   August 2021

 *********************************************************************/

#include "EndToolBoplarHook.h"

#define PI (3.1415926535897932346f)

 /**
  * \brief   EndToolBoplarHook构造函数.
  *
  */
EndToolBoplarHook::EndToolBoplarHook()
{
	//沿着z轴平移0.011米
	m4dTransM1 << 1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0.0104,
		0, 0, 0, 1;

	//绕y轴逆时针旋转90度
	m4dTransM2 << cos(PI / 2), 0, sin(PI / 2), 0,
		0, 1, 0, 0,
		-sin(PI / 2), 0, cos(PI / 2), 0,
		0, 0, 0, 1;

	m4dTransMatrix_1 = m4dTransM1 * m4dTransM2;

	//沿着z轴平移0.36986米

	m4dTransM3 << 1, 0, 0, 0.0081,//0,
		0, 1, 0, -0.00010276033,//0,
		0, 0, 1, dInstrumentLength,
		0, 0, 0, 1;


	m4dTransMatrix = m4dTransM1 * m4dTransM2 * m4dTransM3;
}

/**
 * \brief   EndToolBoplarHook析构函数.
 *
 */
EndToolBoplarHook::~EndToolBoplarHook()
{

}

/**
 * \brief   获取手术器械末端的位姿.
 *
 * \param robotend 机械臂末端位姿
 * \return         末端器械位姿
 */
Eigen::Matrix4d EndToolBoplarHook::geToolEndMatrix(Eigen::Matrix4d m4dRobotend)
{
	return m4dRobotend * m4dTransMatrix;
}




/**
 * \brief   通过RCM创口点约束，将主手位姿加持到器械末端后，求得机械臂末端.
 *
 * \param robotend_current  机械臂当前末端位姿
 * \param toolend_next      末端器械下一位姿
 * \param RCM               RCM点
 * \return                  机械臂下一个末端位姿
 */
Eigen::Matrix4d EndToolBoplarHook::getRobotNextEnd(Eigen::Matrix4d m4dRobotendCurrent, Eigen::Matrix4d m4dToolendNext, Eigen::Vector3d v3dRCM)
{
	//对应于旋转矩阵的xi，yi，zi
	Eigen::Vector3d v3dToolM1_Xi, v3dToolM1_Yi, v3dToolM1_Zi;
	//toolM1为RCM反解时对应的矩阵（反解求机械臂末端），toolend_current时通过机械臂末端矩阵*转换矩阵后，得到的当前器械末端矩阵。
	Eigen::Matrix4d m4dToolM1, m4dToolendCurrent;

	//得到的当前器械末端矩阵
	m4dToolendCurrent = m4dRobotendCurrent * m4dTransMatrix;

	//求手术机械在人体外的长度
	dInstrumentOut = dInstrumentLength - (m4dToolendNext.block(0, 3, 3, 1) - v3dRCM).norm();

	//求当前器械末端到RCM，与目标器械末端到RCM点的差值
	dDistanceOldAndNew = ((v3dRCM - m4dToolendNext.block(0, 3, 3, 1)).norm()) - ((m4dToolendCurrent.block(0, 3, 3, 1) - v3dRCM).norm());

	//计算满足RCM约束下的目标器械末端的姿态
	v3dToolM1_Zi = (m4dToolendNext.block(0, 3, 3, 1) - v3dRCM) / (v3dRCM - m4dToolendNext.block(0, 3, 3, 1)).norm();
	v3dToolM1_Xi = m4dToolendNext.block(0, 0, 3, 1);

	v3dToolM1_Yi = v3dToolM1_Zi.cross(v3dToolM1_Xi);
	v3dToolM1_Yi = v3dToolM1_Yi / v3dToolM1_Yi.norm();
	v3dToolM1_Xi = v3dToolM1_Yi.cross(v3dToolM1_Zi);

	//将目标姿态组成到反解求机械臂末端的矩阵中
	m4dToolM1.block(0, 0, 3, 1) = v3dToolM1_Xi;
	m4dToolM1.block(0, 1, 3, 1) = v3dToolM1_Yi;
	m4dToolM1.block(0, 2, 3, 1) = v3dToolM1_Zi;

	//将目标位置组成到反解求机械臂末端的矩阵中
	m4dToolM1.block(0, 3, 3, 1) = v3dRCM - v3dToolM1_Zi * (dInstrumentOut);
	m4dToolM1.block(3, 0, 1, 4) << 0, 0, 0, 1;

	return m4dToolM1 * m4dTransMatrix_1.inverse();
}

/**
 * \brief   获取机械臂末端手术器械种类.
 *
 * \return  末端器械类型
 */
int EndToolBoplarHook::getEndToolType()
{
    return 2;
}
